
#include "odrive_main.h"


Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
                Config_t& config) :
        hw_config_(hw_config),
        config_(config)
{
    update_pll_gains();
}


void Encoder::setup() {
    //HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
    timer_enable(hw_config_.timer);
    set_idx_subscribe();
    mode_ = config_.mode;
    shadow_count_pll_last_ = shadow_count_;
}
void Encoder::abi_calibration(void) {
    if(abs_spi_pos_updated_ == true && is_ready_ == false && config_.pre_calibrated == true)
    {
        power_on_abs_to_ab_ = pos_abs_;
        abs_spi_pos_updated_ = false;
        is_ready_ = true;
        set_linear_count(power_on_abs_to_ab_);
    }
}

void Encoder::set_error(Error error) {
    vel_estimate_valid_ = false;
    pos_estimate_valid_ = false;
    error_ = error;
    axis_->error_ = Axis::ERROR_ENCODER_FAILED;
}

bool Encoder::do_checks(){
    return error_ == ERROR_NONE;
}

//--------------------
// Hardware Dependent
//--------------------

// Triggered when an encoder passes over the "Index" pin
// TODO: only arm index edge interrupt when we know encoder has powered up
// (maybe by attaching the interrupt on start search, synergistic with following)
void Encoder::enc_index_cb() {
    ;
}

void Encoder::set_idx_subscribe(bool override_enable) {
	//GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);

}

void Encoder::update_pll_gains() {
    pll_kp_ = 2.0f * config_.bandwidth;    // basic conversion to discrete time
    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped

    // Check that we don't get problems with discrete time approximation
    if (!(current_meas_period * pll_kp_ < 1.0f)) {
        set_error(ERROR_UNSTABLE_GAIN);
    }
}

void Encoder::check_pre_calibrated() {
    ;
}

// Function that sets the current encoder count to a desired 32-bit value.
void Encoder::set_linear_count(int32_t count) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = cpu_enter_critical();
	
	count_in_cpr_ = count;
	pos_cpr_counts_ = count;
	pos_cpr_ = pos_estimate_;
	pos_circular_ = pos_cpr_;

    // Update states
    shadow_count_ = count;
    pos_estimate_counts_ = (float)count;
    tim_cnt_sample_ = count;
    
    //Write hardware last

    //4096
	//hw_config_.timer->Instance->CNT = count;//;
    TIMER_CNT(hw_config_.timer) = count;
	//16384
	//hw_config_.timer->Instance->CNT = (count*4);  
	
	config_.mode = MODE_INCREMENTAL;
	mode_ = MODE_INCREMENTAL;    

	cpu_exit_critical(prim);
	
}

// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
// Note that this will get mod'ed down to [0, cpr)
void Encoder::set_circular_count(int32_t count, bool update_offset) {
    // Disable interrupts to make a critical section to avoid race condition
    uint32_t prim = cpu_enter_critical();

    if (update_offset) {
        config_.offset += count - count_in_cpr_;
        config_.offset = mod(config_.offset, config_.cpr);
    }

    // Update states
    count_in_cpr_ = mod(count, config_.cpr);
    pos_cpr_counts_ = (float)count_in_cpr_;

    cpu_exit_critical(prim);
}

// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
    const float start_lock_duration = 1.0f;
    const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);

    // We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_
    // Therefore we have to sync them for calibration
    shadow_count_ = count_in_cpr_;

    float voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;

    // go to motor zero phase for start_lock_duration to get ready to scan
    int i = 0;
    axis_->run_control_loop([&](){
        if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
        return ++i < start_lock_duration * current_meas_hz;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    int32_t init_enc_val = shadow_count_;
    int64_t encvaluesum = 0;
    set_linear_count(0);
    // scan forward
    i = 0;
    axis_->run_control_loop([&]() {
        float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);

		if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
            
        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;
        
        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    // Check response and direction
    if (shadow_count_ > init_enc_val + 8) {
        // motor same dir as encoder
        axis_->motor_.config_.direction = 1;
    } else if (shadow_count_ < init_enc_val - 8) {
        // motor opposite dir as encoder
        axis_->motor_.config_.direction = -1;
    } else {
        // Encoder response error
        set_error(ERROR_NO_RESPONSE);
        return false;
    }

    //TODO avoid recomputing elec_rad_per_enc every time
    // Check CPR
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));  /*  */
    float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;                         /*  */
    
    calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);                                         /*  */
    if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) { /*  */
        set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
        return false;
    }

    // scan backwards
    i = 0;
    axis_->run_control_loop([&]() {
        float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
            return false; // error set inside enqueue_voltage_timings
        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);

        encvaluesum += shadow_count_;
        
        return ++i < num_steps;
    });
    if (axis_->error_ != Axis::ERROR_NONE)
        return false;

    config_.offset = encvaluesum / (num_steps * 2);
    int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
    config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f;  // add 0.5 to center-align state to phase

    printf("encoder is ok\r\n");
    printf("offest:%d\r\n",config_.offset);
    is_ready_ = true;
    return true;
}

//static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
//    return true;
//}

void Encoder::sample_now() {
    switch (mode_) {
        case MODE_INCREMENTAL: {
			  tim_cnt_sample_ = (int16_t)(TIMER_CNT(hw_config_.timer)); //0~65534 = 4rad		
        } break;
	    case MODE_ABS_PWM:
        {
            axis_->motor_.log_timing(TIMING_LOG_SAMPLE_NOW);
            // Do nothing
        } break;

        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
        } break;
    }
    
}

bool Encoder::abs_spi_init(){

    return false;
	
}

bool Encoder::abs_spi_start_transaction(){

    return true;
	
}

uint8_t ams_parity(uint16_t v) {
    v ^= v >> 8;
    v ^= v >> 4;
    v ^= v >> 2;
    v ^= v >> 1;
    return v & 1;
}

uint8_t cui_parity(uint16_t v) {
    v ^= v >> 8;
    v ^= v >> 4;
    v ^= v >> 2;
    return ~v & 3;
}

void Encoder::abs_spi_cb(){
    ;

}

void Encoder::abs_spi_cs_pin_init(){
    ;
}

bool Encoder::update() {
    // update internal encoder state.
    
    int32_t delta_enc = 0;
    int32_t pos_abs_latched = pos_abs_; //LATCH
    
    switch (mode_) {
        case MODE_INCREMENTAL: {
            int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;	
            delta_enc = (int32_t)delta_enc_16; //sign extend
		} break;
		case MODE_ABS_PWM: {
			#if 0
            if (abs_spi_pos_updated_ == false) {
				
                // Low pass filter the error
                spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
                if (spi_error_rate_ > 0.005f)
                    set_error(ERROR_ABS_SPI_COM_FAIL);
            } else {
                // Low pass filter the error
                spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
            }
           #endif
            //abs_spi_pos_updated_ = false;
            delta_enc = pos_abs_latched - count_in_cpr_; //LATCH
            delta_enc = mod(delta_enc, config_.cpr);
            if (delta_enc > config_.cpr/2) {
                delta_enc -= config_.cpr;
            }

        }break;
        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
           return false;
        } break;
    }
    shadow_count_ += delta_enc;
    count_in_cpr_ += delta_enc;
    count_in_cpr_ = mod(count_in_cpr_, config_.cpr);

	
    //// run pll (for now pll is in units of encoder counts)
    // Predict current pos
    pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
    pos_cpr_counts_      += current_meas_period * vel_estimate_counts_;
	
    // discrete phase detector
    float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
    float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));
    delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
	
    // pll feedback
    pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
	
    pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
    pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
    vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
	


    bool snap_to_zero_vel = false;
    if (std::abs(vel_estimate_counts_) < 1.0f * current_meas_period * pll_ki_) {
        vel_estimate_counts_ = 0.0f;  //align delta-sigma on zero to prevent jitter
        snap_to_zero_vel = true;
    }

    // Outputs from Encoder for Controller
    float pos_cpr_last = pos_cpr_;	

    //--------------------------------------------------------
    pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
    vel_estimate_ = (vel_estimate_counts_ / (float)config_.cpr);

    pos_cpr_= pos_cpr_counts_ / (float)config_.cpr;
    float delta_pos_cpr = wrap_pm(pos_cpr_ - pos_cpr_last, 0.5f);
    pos_circular_ += delta_pos_cpr;
    pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range);

    //// run encoder count interpolation
    int32_t corrected_enc = count_in_cpr_ - config_.offset;
    // if we are stopped, make sure we don't randomly drift
    if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
        interpolation_ = 0.5f;
    // reset interpolation if encoder edge comes
    // TODO: This isn't correct. At high velocities the first phase in this count may very well not be at the edge.
    } else if (delta_enc > 0) {
        interpolation_ = 0.0f;
    } else if (delta_enc < 0) {
        interpolation_ = 1.0f;
    } else {
        // Interpolate (predict) between encoder counts using vel_estimate,
        interpolation_ += current_meas_period * vel_estimate_counts_;
        // don't allow interpolation indicated position outside of [enc, enc+1)
        if (interpolation_ > 1.0f) interpolation_ = 1.0f;
        if (interpolation_ < 0.0f) interpolation_ = 0.0f;
    }
    float interpolated_enc = corrected_enc + interpolation_;

    //// compute electrical phase
    //TODO avoid recomputing elec_rad_per_enc every time
    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));

    float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
    // ph = fmodf(ph, 2*M_PI);
    phase_ = wrap_pm_pi(ph);
    vel_estimate_valid_ = true;
    pos_estimate_valid_ = true;
    if(axis_->axis_num_ ==1)
    {
        if(encoder_times_20ms_++==16)
        {
            //printf("A%f\r\n",phase_*57.2957);
            //printf("",count_in_cpr_);
            //printf("%d  tor:%f vel_e:%f setVel:%f\r\n",count_in_cpr_,phase_*57.2957,vel_estimate_,axis_->controller_.vel_setpoint_);
            //printf("A:%f  B%f\r\n",vel_estimate_,axis_->controller_.vel_setpoint_);
            encoder_times_20ms_=0;
        }
    }
    
    return true;
}
